/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    GaitScheduler.h
  * @author  Silang Wu
  * @brief   Schedule the gait pattern for 4 legs independently in Body frame.
  * @note    The following gait is supported for now:
  *          - Stand Up/Down, Walk, Trot, Pace, Gallop.
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
#ifndef GAIT_SCHEDULER_H
#define GAIT_SCHEDULER_H
/* Includes ------------------------------------------------------------------*/
#include "Utilities/additionTypes.h"
#include "Utilities/common_def.h"
#include <math.h>
/* Values define --------------------------------------------------------------*/
#define PI M_PI
/* Types define --------------------------------------------------------------*/
namespace Quadruped
{
/**
 * @brief Predefine supported gait type.
 */
enum class GaitType
{
    STAND_UP,
    STAND_DOWN,
    WALK,
    TROT,
    PACE,
    CANTER,
    GALLOP,
    JUMP,
    BOUNCE
};

/**
 * @brief Define supported trajectory types.
 */
enum TrajectoryType
{
    Sinusoidal_Trajectory = 0,
    LPI_Trajectory,
    Polynomial_Trajectory,
    Cycloid_Trajectory
};
enum ContactState
{
    stanceState = 0,
    swingState = 1
};

enum TrajSchedulerType
{
    jointSpace = 0,
    cartesianSpace
};
/**
 * @brief Gait data for scheduler.
 */
template <typename T>
struct GaitData
{
    /*========================= Gait Parameters ============================*/
    //!< local storage.
    GaitType current_gait;
    GaitType next_gait;
    Vec4<T> _swingTime;
    Vec4<T> _stanceTime;
    Vec4<T> _swingPhase;
    Vec4<T> _stancePhase;
    Vec4<T> _swingProportion;
    Vec4<T> _stanceProportion;
    Vec4<T> _phaseOffset;
    Vec4<T> _stanceTimeRemaining;//here "Time" means "remaining proportion".
    Vec4<T> _swingTimeRemaining;//here "Time" means "remaining proportion".
    Vec4<T> _phaseInStance;
    Vec4<T> _PreExeStanceFlag;
    Vec4<T> _DelayExeStanceFlag;

    T initialPhase;
    T exeBoundJumpTime;
    T exeRetractTime;
    T exeStopRetractTime;
    T exeFallTime;
    T endWholeJumpTime;
    int _gaitChangeFlag;
    int _exeStanceEndFlag;
    int _endBeforeJumpFlag;
    int _endBoundJumpFlag;
    int _endRetractFlag;
    int _endJumpFlag;
    int _exeTrotToCanterFlag;
    int _runTimes;
    int _startSignal;
    /*========================= Trajectory Parameters ============================*/
    Vec3<T>   _pStart[4];//the final position in the trajectory.
    Vec3<T>   _pFinal[4];//the starting position in the trajectory.
    Vec3<T>   _pCanterStretch[4];
    Vec3<T>   _pInit[4];//the initial position when robot stands up.
    Vec3<T>   _pBeforeStand[4];//the position before standing up.
    Vec4<T>   _trajHgt;
    Vec4<T>   _overflowFlag;
    Vec4<T>   _swingEndFlag;
    Vec4<T>   _trajFootState;
    Vec4<T>   _RealFootState;
    T         _dPhase;
    T         _phase;
    T         _StandupExeTime;
    T         _StandupExePeriodTime;
    T         _exedt;
    T         _sampleFactor;
    T         _interPolationPropotion;
    Vec3<T>   _vBody;
    Vec3<T>   _v_CoM;
    Vec3<T> _pdelta;
    Vec3<T> _trajP[4];
    Vec3<T> _trajV[4];
    Vec3<T> _trajA[4];

    /**
     * This matrix is used to show trajectory information 
     * including foot positions and execution time in the period of each foot.
    */
    Mat4<T> _trajInfo;
    int _trajScheType;
    int _trajType;
    int _cycleIndex;
    int _halfcycleIndex;
    
    T _periodTime;
    /*
    * command for position/velocity/acceration of foot point:
    * literally,the data at the point when swing phase turn to hold phase.
    */
    Vec3<T> pFoot_cmd[4];
    Vec3<T> vFoot_cmd[4];
    Vec3<T> aFoot_cmd[4];

    /*
    * currunt position/velocity/acceration of foot point:
    * literally,the data in the moment.
    */
    Vec3<T> pFoot_now[4];
    Vec3<T> vFoot_now[4];
    Vec3<T> aFoot_now[4];

    GaitData() { zero(); }

    void zero()
    {
        next_gait = current_gait;
        _halfcycleIndex = 0;
        _phase=0.0;
        _gaitChangeFlag=0;
        _exeStanceEndFlag =0;
        _endBeforeJumpFlag=0;
        _endBoundJumpFlag=0;
        _exeTrotToCanterFlag=0;
        _endJumpFlag=0;
        _periodTime = 0.0;
        _runTimes=0;
        _sampleFactor=0;
        _interPolationPropotion=0;
        _swingTime.setZero();
        _stanceTime.setZero();
        _swingPhase.setZero();
        _stancePhase.setZero();
        _swingProportion.setZero();
        _stanceProportion.setZero();
        _phaseOffset.setZero();
        _stanceTimeRemaining.setZero();
        _swingTimeRemaining.setZero();
        _overflowFlag.setZero();
        _trajFootState.setZero();
        _DelayExeStanceFlag.setZero();
        _PreExeStanceFlag.setZero();
        _startSignal=4;
    }
};

template <class T>
class GaitScheduler
{
public:
    GaitScheduler(/* args */);
    ~GaitScheduler();

    /*============================= Gait Functions ==============================*/

    void initialize();
    void createGait(GaitType gait_type);
    bool changeGait(GaitType gait_type);//Support transition among walk, trot, canter & gallop.
    void gaitGenerate();//only support walk
    /*========================= Trajectory Functions ============================*/

    
    const Vec3<T>& GetFootPos(int foot_id);
    const Vec3<T>& GetFootVel(int foot_id);
    const Vec3<T>& GetFootAcc(int foot_id);
    const Vec3<T>& GetVBody();
    int     GetPeriodEndFlag();
    T       GetPeriod();
    int     GetSwingEndFlag(int foot_id);
    int     GetFootState(int foot_id);
    int     GetRealFootState(int foot_id);
    int     GetExeStanceFlag();
    int     GetEndBeforeJumpFlag();
    int     GetEndBoundJumpFlag();
    int     GetEndRetractFlag();
    int     GetEndFallToGroundFlag();
    int     GetEndWholeJumpFlag();
    int     GetGaitChangeFlag();
    T       GetCurrentPhase();
    T       GetPhaseInStance(int foot_id);
    T       GetStanceTime(int foot_id);
    T       GetCurrentDPhase();
    Vec3<T>       GetDebugv_d();
    GaitType GetNextGaitType();
    GaitType GetCurrentGaitType();
    void    TrajParamSetZero();
    void    Traj_Param_Init(T traj_height, int traj_type, int trajSche_type);
    //Functions about settings
    void    SetTrajHeight(T traj_height,int foot_id);
    void    SetTrajType(int traj_type);
    void    SetPhaseInterval(T dPhase);
    void    SetExeBoundJumpTime(T boundTime);
    void    SetExeRetractTime(T exeRetractTime);
    void    SetStopExeRetractTime(T stopExeRetractTime);
    void    SetFallTime(T stfall);
    void    SetEndWholeJumpTime(T endWholeJumpTime); 
    void    SetSampleFactor(T factor);
    void    SetCoMVelocity(T vx, T vy, T vz);
    void    SetInterPolationPorpotion(T propotion);
    void    SetFootPlacement(const Vec3<T> &startPoint, const Vec3<T> &finalPoint,T trajHeight,int foot_id);
    
    //Functions about trajectory.
    void    SinTraj(int foot_id);
    void    PolyTraj(int foot_id);
    void    StartTraj(int foot_id);
    void    CanterTraj(int foot_id);
    /*print trajectory information.*/
    void    ShowInfo();

    /*Functions about gait transition*/
    void    TransitionToWalk();
    void    TransitionToTrot();
    void    TransitionToCanter();
    void    TransitionToGallop();
    void    TransitionToStand();
    void    TransitionToJump();
    void    TransitionToBounce();

    void    ClearJumpFlag();

    void    JudgeFootState(ContactState outsideInput,int id);
private:
    GaitData<T> gaitData;
};

} // namespace Quadruped

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/

